/**
 * 工具包功能实现
 */

#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "utils.h"


 // 调试用的标志
static size_t _dbgflags = 0;

void _dbgflag_set(unsigned int idx, bool val) {
	if (idx >= sizeof(_dbgflags) * 8) return;
	if (val) { _dbgflags |= (((size_t)1) << idx); return; }
	_dbgflags &= ~(((size_t)1) << idx);
}

bool _dbgflag_get(unsigned int idx) {
	if (idx >= sizeof(_dbgflags) * 8) return 0;
	return (_dbgflags >> idx) & 1;
}

size_t memcpy_to_aligned32(void *dst32, const void *_src, size_t _size) {
	if(!dst32 || !_src || !_size) return 0;
	size_t addr = (size_t)dst32;
	size_t size = _size;
	const uint8_t *src = (const uint8_t*)_src;

	// 如果起始位置未 uint32_t 对齐，则先写入未对齐部分数据
	uint32_t residue = addr % 4;
	if(residue) {
		int nfill = 4 - residue; // 未对齐的字节数
		if(size < nfill) nfill = size; // 需要补齐的字节数
		addr &= 0xFFFFFFFFFFFFFFFC;
		uint32_t reg = *((uint32_t*)addr);
		if(nfill >= 1) ((char*)&reg)[residue + 0] = ((char*)src)[0];
		if(nfill >= 2) ((char*)&reg)[residue + 1] = ((char*)src)[1];
		if(nfill >= 3) ((char*)&reg)[residue + 2] = ((char*)src)[2];
		*((uint32_t*)addr) = reg;
		addr += 4; src += nfill; size -= nfill;
	}

	// 写入 uint32_t 对齐部分数据
	size_t nreg = size / 4;
	for(size_t i=0; i < nreg; i++, addr += 4, src += 4) {
		*((uint32_t*)addr) = *((uint32_t*)src);
	}

	// 如果结束位置未 uint32_t 对齐，则写入未对齐部分数据
	residue = size - nreg * 4; // 需要补齐的字节数
	if(residue) {
		uint32_t reg = *((uint32_t*)addr);
		if(residue >= 1) ((char*)&reg)[0] = ((char*)src)[0];
		if(residue >= 2) ((char*)&reg)[1] = ((char*)src)[1];
		if(residue >= 3) ((char*)&reg)[2] = ((char*)src)[2];
		*((uint32_t*)addr) = reg;
	}

	return _size;
}

size_t memcpy_from_aligned32(void *_dst, const void *src32, size_t _size) {
	if(!_dst || !src32 || !_size) return 0;
	size_t addr = (size_t)src32;
	size_t size = _size;
	const uint8_t *dst = (const uint8_t*)_dst;

	// 如果起始位置未 uint32_t 对齐，则先写入未对齐部分数据
	uint32_t residue = addr % 4;
	if(residue) {
		int nfill = 4 - residue; // 未对齐的字节数
		if(size < nfill) nfill = size; // 需要补齐的字节数
		addr &= 0xFFFFFFFFFFFFFFFC;
		uint32_t reg = *((uint32_t*)addr);
		if(nfill >= 1) ((char*)dst)[0] = ((char*)&reg)[residue + 0];
		if(nfill >= 2) ((char*)dst)[1] = ((char*)&reg)[residue + 1];
		if(nfill >= 3) ((char*)dst)[2] = ((char*)&reg)[residue + 2];
		addr += 4; dst += nfill; size -= nfill;
	}

	// 写入 uint32_t 对齐部分数据
	size_t nreg = size / 4;
	for(size_t i=0; i < nreg; i++, addr += 4, dst += 4) {
		*((uint32_t*)dst) = *((uint32_t*)addr);
	}

	// 如果结束位置未 uint32_t 对齐，则写入未对齐部分数据
	residue = size - nreg * 4; // 需要补齐的字节数
	if(residue) {
		uint32_t reg = *((uint32_t*)addr);
		if(residue >= 1) ((char*)dst)[0] = ((char*)&reg)[0];
		if(residue >= 2) ((char*)dst)[1] = ((char*)&reg)[1];
		if(residue >= 3) ((char*)dst)[2] = ((char*)&reg)[2];
	}

	return _size;
}
